#include <iostream>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <cmath>
#define PI 3.1415926

using namespace std;
using namespace cv;
using namespace Eigen;

int main()
{
	//Eigen
	//虽然称为3d(指3维空间的欧式坐标)，实质上是4*4的矩阵，旋转R+平移T
	//绕z轴旋转30度
	Isometry3d Trans_1 = Isometry3d::Identity();
	AngleAxisd rotation_vector1(PI/6, Vector3d(0, 0, 1));
	Trans_1.rotate (rotation_vector1);
	//平移向量(2,-3,1)
	Isometry3d Trans_2 = Isometry3d::Identity();
	Trans_2.pretranslate(Vector3d(2, -3, 1));
	//绕y轴旋转45度
	Isometry3d Trans_3 = Isometry3d::Identity();
	AngleAxisd rotation_vector3(PI/4, Vector3d(0, 1, 0));
	Trans_3.rotate (rotation_vector3);
	//绕z轴旋转60度
	Isometry3d Trans_4 = Isometry3d::Identity();
	AngleAxisd rotation_vector4(PI/3, Vector3d(1, 0, 0));
	Trans_4.rotate (rotation_vector4);
	//初始坐标(1,2,0)
	Vector3d Vec_0(1, 2, 0);
	//变换后坐标(1,2,0)
	Vector3d Vec_1 = Trans_3 * Trans_1 * Trans_2 * Trans_4 * Vec_0;
	//输出变换后的坐标
	cout<< "(Eigen)Result = \n" << Vec_1 <<endl;


	//OpenCV
	//绕z轴旋转30度
	Mat T_1=(Mat_<float>(4, 4)<<
		cos(PI/6), -sin(PI/6), 0, 0, 
		sin(PI/6), cos(PI/6), 0, 0, 
		0, 0, 1, 0, 
		0, 0, 0, 1);
	//平移向量(2,-3,1)
	Mat T_2=(Mat_<float>(4, 4)<<
		1, 0, 0, 2,
		0, 1, 0, -3,
		0, 0, 1, 1,
		0, 0, 0, 1);
	//绕y轴旋转45度
	Mat T_3=(Mat_<float>(4, 4)<<
		cos(PI/4), 0, sin(PI/4), 0,
		0, 1, 0, 0,
		-sin(PI/4), 0, cos(PI/4), 0,
		0, 0, 0, 1);
	//绕z轴旋转60度
	Mat T_4=(Mat_<float>(4, 4)<<
		1, 0, 0, 0,
		0, cos(PI/3), -sin(PI/3), 0,
		0, sin(PI/3), cos(PI/3), 0,
		0, 0, 0, 1);
	//初始坐标(1,2,0)
	Mat V_0=(Mat_<float>(4, 1)<<1, 2, 0, 1);
	//输出变换后的坐标
	cout << "(OpenCV)Result = \n" << T_3*T_1*T_2*T_4*V_0 << endl;

	return 0;
}

